In [1]:
%pylab inline
import control
import pyhull
import string
import picos as pic
import cvxopt as cvx
import numpy as np
import scipy.linalg
import scipy.optimize
import scipy.integrate
import itertools
import sympy
import sympy.physics.mechanics as me
from matplotlib.patches import Ellipse
In [2]:
rcParams['lines.linewidth'] = 2
rcParams['font.size'] = 12
rcParams['animation.writer'] = 'avconv'
rcParams['animation.codec'] = 'libx264'
The $L_\infty$ gain characterizes the ratio of the maximum values of the output to the input. This is useful for computing th reachable set for the output. The reachable set can then be used in formal analysis to verify system safety.
source: AAE 666 Class Notes (Martin Corless) pg. 178/ pg. 265
Consider a disturbed linear system:
\begin{align*} \dot{x} = Ax + Bw\\ z = Cx + Dw \end{align*}where all eigenvalues of A have negative real part and w is a bounded input. Suppose there exists a positive real scalar $\alpha$ such that:
\begin{align*} \begin{pmatrix} PA + A^TP + 2\alpha P & PB\\ B^TP & -2\alpha \mu_1 I \end{pmatrix} < 0\\ \begin{pmatrix} C^TC - P & C^T D \\ D^TC & D^TD - \mu_2 I \end{pmatrix} < 0 \end{align*}Then $||y(t)||_{\infty} \leq \sqrt{\beta^2 e^{-2 \alpha t} + \gamma ||u(t)||_{\infty} }$ where
\begin{align*} \beta &= \sqrt{x_0^TPx_0}\\ \gamma &= \sqrt{\mu_1 + \mu_2} \end{align*}Since $\alpha$ and P cannot both be a variable in the LMI in order for it to be linear, a line search must be performed to minimize $\gamma$ by changing $\alpha$. It is beneficial to minimize $\gamma$ since it is the bound at steady state while $\alpha$ represents the transient behaviour.
In [3]:
def solve_bounded_disturbance(sys):
A = sys.A
B = sys.B
C = sys.C
D = sys.D
def solve_lmi(A_data, B_data, C_data, D_data, alpha, verbose=False):
sdp = pic.Problem()
# shape
n_x = A_data.shape[0]
n_u = B_data.shape[1]
# variables
P = sdp.add_variable('P', (n_x, n_x), vtype='symmetric')
alpha = pic.new_param('alpha', alpha)
mu_1 = sdp.add_variable('mu_1')
mu_2 = sdp.add_variable('mu_2')
# parameters
A = pic.new_param('A', cvx.matrix(A_data))
B = pic.new_param('B', cvx.matrix(B_data))
C = pic.new_param('C', cvx.matrix(C_data))
D = pic.new_param('D', cvx.matrix(D_data))
I_n_u = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_u))))
I_n_x = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_x))))
eps = 1e-10
sdp.add_constraint(
(P*A + A.T*P + 2*alpha*P & P*B) //
(B.T*P & -2*alpha*mu_1*I_n_u) << -eps)
sdp.add_constraint(
(C.T*C - P & C.T*D) //
(D.T*C & D.T*D - mu_2*I_n_u) << -eps)
sdp.add_constraint(P >> eps*I_n_x)
sdp.add_constraint(mu_1 >> eps)
sdp.add_constraint(mu_2 >> eps)
sdp.set_objective('min', mu_1 + mu_2)
try:
sdp.solve(verbose=verbose)
mu_1 = sdp.variables['mu_1'].value[0,0]
mu_2 = sdp.variables['mu_2'].value[0,0]
gam = np.sqrt(mu_1 + mu_2)
except Exception as e:
print e
gam = -1
return gam, sdp
# we use fmin to solve a line search problem in alpha for minimum gamma
print('line search')
alpha_opt = scipy.optimize.fmin(lambda alpha: solve_lmi(A, B, C, D, alpha)[0], x0=0.01)
gam, sdp = solve_lmi(A, B, C, D, alpha_opt)
print sdp
if sdp.status == 'optimal':
P = sdp.variables['P'].value
mu_1 = sdp.variables['mu_1'].value[0,0]
mu_2 = sdp.variables['mu_2'].value[0,0]
print 'optimal alpha: ', alpha_opt
print 'gamma: ', gam
print 'mu_1: ', mu_1
print 'mu_2: ', mu_2
print 'P: ', P
else:
sdp = solve_lmi(A, B, C, D, alpha_opt, verbose=True)
raise RuntimeError('Optimization failed')
return sdp, P, alpha_opt, gam
In [4]:
class MyConvexHull(object):
def __init__(self, res=None, eqs=None, points=None, facets=None, max_dist=None):
self.res = res
self.eqs = eqs
self.points = points
self.facets = facets
self.max_dist = max_dist
@classmethod
def from_points(cls, points, angle=0.99):
res = pyhull.qconvex('n i A{:f} Fs'.format(angle), points)
n_dim = int(res[0])-1
n_eqs = int(res[1])
eqs = array([line.split() for line in res[2:2+n_eqs]]).astype(float)
n_facets = int(res[2+n_eqs])
facets = array([line.split() for line in res[3+n_eqs:3+n_eqs+n_facets]]).astype(int)
max_dist = float(res[4+n_eqs+n_facets].split()[1])
return cls(res, eqs, points, facets, max_dist)
@classmethod
def from_halfspaces(cls, interior_point, halfspaces):
s = 'H'
for i in range(len(interior_point)):
s += '{:5g},'.format(interior_point[i])
opt = s + ' Fp'
res = pyhull.qhull_cmd('qhalf', opt, halfspaces)
n_dim = int(res[0])
n_vert = int(res[1])
points = array([line.split() for line in res[2:2+n_vert]]).astype(float)
return cls.from_points(points)
def add_buffer(self, b):
self.eqs[:,-1] -= b
s = zeros(self.points[0].shape)
interior_point = self.points.mean(0)
return MyConvexHull.from_halfspaces(interior_point, self.eqs)
def __repr__(self):
return string.join(self.res, '\n')
def contains(self, p):
return False
In [5]:
def create_flow_tube(mode, x_0, u_norm):
x_t = mode['x_t']
beta = mode['beta']
alpha = mode['alpha']
gam = mode['gam']
sys = mode['sys_b']
t = linspace(0,20,1000)
u = array([[0,x_t[0],x_t[1]]]).T.dot(ones((1,len(t))))
x0 = array([x_0[0], x_0[1], x_0[0], x_0[1]])
bound = sqrt(beta**2*exp(-2*alpha*t) + (u_norm*gam)**2).T
t, y, x = control.forced_response(sys, T=t, U=u, X0=x0)
nom = y[2:4,:].T # nominal trajecotry
#bounds
p = array([
nom + bound.dot(array([[1,1]])),
nom + bound.dot(array([[-1,1]])),
nom + bound.dot(array([[1,-1]])),
nom + bound.dot(array([[-1,-1]]))
])
# space using arclength
n_steps = 10
arc_length = cumsum(norm(array([diff(nom[:,0]),diff(nom[:,1])]), axis=0))
norm_arc_length = arc_length/arc_length[-1]
i_steps = [find(norm_arc_length >= per)[0] for per in linspace(0,1,n_steps)]
# find convex sets
flow_tube = []
for i in range(n_steps-1):
i0 = i_steps[i]
i1 = i_steps[i+1]+1
step = i1-i0
p_i = reshape(p[:,i0:i1,:], [p.shape[0]*step,p.shape[2]])
ch = MyConvexHull.from_points(p_i, angle=0.9)
ch = ch.add_buffer(ch.max_dist)
flow_tube.append(ch)
return flow_tube, nom
In [6]:
def derive_pend_eoms():
theta, t, m, g, l = sympy.symbols('theta, t, m, g, l')
values = {'g': 9.8, 'l': 0.4, 'm': 1}
frame_i = me.ReferenceFrame('i') # the inertial frame
frame_b = frame_i.orientnew('b', 'Axis', (theta(t), frame_i.z)) # the body frame
point_o = me.Point('o') # the base of the pendulum
point_o.set_vel(frame_i, 0) # the base of the pendulum is fixed in the inertial frame
point_p = point_o.locatenew('p', l*frame_b.y) # the end of the pendulum
point_p.set_vel(frame_b, 0) # point p is fixed in the body frame
point_p.v1pt_theory(point_o, frame_i, frame_b)
particle_a = me.Particle('a', point_p, m) # define a point mass particle at the end of the pendulum
weight = -m*g*frame_i.y
weight
u = sympy.Symbol('u') # input
d = sympy.Symbol('d') # disturbance
M_o = point_p.pos_from(point_o).cross(weight).dot(frame_b.z) + u(t) + d(t)
H__i_o = particle_a.angular_momentum(point_o, frame_i).dot(frame_i.z)
eom = M_o - H__i_o.diff(t)
eom_theta_dd = sympy.solve(eom, theta(t).diff(t,2))[0]
eom_equil = eom.subs({theta(t).diff(t,2):0})
u_eq = sympy.solve(eom.subs({theta(t).diff(t,2):0}), u(t), theta(t))[0][u(t)]
x_vect = sympy.Matrix([theta(t), theta(t).diff(t)])
u_vect = sympy.Matrix([u(t), d(t)])
y_vect = x_vect
f_vect = sympy.Matrix([theta(t).diff(t), eom_theta_dd])
f_vect
A_eq = f_vect.jacobian(x_vect)
B_eq = f_vect.jacobian(u_vect)
C_eq = y_vect.jacobian(x_vect)
D_eq = y_vect.jacobian(u_vect)
return A_eq, B_eq, C_eq, D_eq, u_eq, values
In [7]:
def linearize_pend(theta_0, A_eq, B_eq, C_eq, D_eq, u_eq, values):
t, d, theta = sympy.symbols('t, d, theta')
u0 = u_eq.subs(values).subs(d(t),0).subs(theta(t), theta_0)
x0 = array([[theta_0, 0]]).T
equil = {theta(t):theta_0}
equil.update(values)
ss_eq = [ np.array(np.array(mat.subs(equil)), dtype=float) for mat in [A_eq,B_eq,C_eq,D_eq]]
return control.StateSpace(ss_eq[0], ss_eq[1], ss_eq[2], ss_eq[3]), x0, u0
In [8]:
def design_lqr(sys):
K, S, E = control.lqr(sys.A,sys.B[:,0],diag([1, 1]), np.eye(1))
closed_loop_lqr = sys[:,0].feedback(K)
return closed_loop_lqr, K
In [9]:
def lmi_design(theta_0):
A_eq, B_eq, C_eq, D_eq, u_eq, values = derive_pend_eoms()
sys, x0, u0 = linearize_pend(theta_0, A_eq, B_eq, C_eq, D_eq, u_eq, values)
closed_loop_lqr, K_lqr = design_lqr(sys)
A = closed_loop_lqr.A
B = closed_loop_lqr.B
Ab = bmat([[A, zeros((2,2))],
[zeros((2,2)), A]])
Bb = bmat([[B, -A],
[zeros((2,1)), -A]])
Cb = bmat([[eye(4)],
[eye(2), -eye(2)]])
Db = zeros((6,3))
sys_e = control.ss(A, B, eye(2), zeros((2,1))) # error system
sys_b = control.ss(Ab, Bb, Cb, Db); # complete system
u_norm = 0.1 # norm of disturbance
d = 0.5 # size of step in pitch angle
sdp, P, alpha, gam = solve_bounded_disturbance(sys_e)
e0 = array([[0.02, 0.02]]).T # initial error, sets ellipse size
beta = sqrt(e0.T.dot(P).dot(e0))
beta
return{
'sys_b': sys_b,
'x_t': x0,
'u0': u0,
'alpha': alpha,
'beta': beta,
'gam': gam,
'P': P
}
In [10]:
u_norm = 0.01
d = 0.5
mode_stop = lmi_design(0)
mode_bwd = lmi_design(-d)
mode_fwd = lmi_design(d)
flow_tube_stop2fwd, nom_stop2fwd = create_flow_tube(mode_fwd, [0,0], u_norm)
flow_tube_stop2bwd, nom_stop2bwd = create_flow_tube(mode_bwd, [0,0], u_norm)
flow_tube_fwd2stop, nom_fwd2stop = create_flow_tube(mode_stop, [d,0], u_norm)
flow_tube_fwd2bwd, nom_fwd2bwd = create_flow_tube(mode_bwd, [d,0], u_norm)
flow_tube_bwd2stop, nom_bwd2stop = create_flow_tube(mode_stop, [-d,0], u_norm)
flow_tube_bwd2fwd, nom_bwd2fwd = create_flow_tube(mode_fwd, [-d, 0], u_norm)
nom_traj = [
nom_stop2fwd,
nom_stop2bwd,
nom_fwd2stop,
nom_fwd2bwd,
nom_bwd2stop,
nom_bwd2fwd
]
flow_tubes = [
flow_tube_stop2fwd,
flow_tube_stop2bwd,
flow_tube_fwd2stop,
flow_tube_fwd2bwd,
flow_tube_bwd2stop,
flow_tube_bwd2fwd,
]
In [11]:
def create_ellipse(mode):
P = mode['P']
beta = mode['beta']
x_t = mode['x_t']
lam, v = eig(P)
height = sqrt(lam[0])*2*beta
width = sqrt(lam[1])*2*beta
angle = rad2deg(np.arccos(v[0, 0]))
return Ellipse(xy=x_t, width=width, height=height, angle=angle, alpha=0.2)
In [12]:
figure(figsize(15,5))
ax = subplot(111)
h_e = ax.add_artist(create_ellipse(mode_fwd))
ax.add_artist(create_ellipse(mode_bwd))
ax.add_artist(create_ellipse(mode_stop))
color_cycle = itertools.cycle(['g', 'b', 'c', 'm', 'y', 'k'])
for flow_tube, nom in zip(flow_tubes, nom_traj):
color = next(color_cycle)
h_nom = ax.plot(nom[:,0], nom[:,1], color=color, linestyle='-')
for ch in flow_tube:
for facet in ch.facets:
h_ch = ax.plot(ch.points[facet][:,0], ch.points[facet][:,1], color=color, linestyle='--')
title('flow tubes')
xlabel('$\\theta$, rad')
ylabel('$\dot{\\theta}$, rad/s')
theta = linspace(-1,1)
a = axis()
#plot(theta, theta*K[0,1]/K[0,0] + 2, 'r-*')
#h_bound = plot(theta, theta*K[0,1]/K[0,0] - 2, 'r-*')
#axis([-d*1.5, d*1.5, -2, 2])
legend([h_nom[0], h_ch[0], h_e], ['nominal', 'flow tube', 'invariant state'], loc='best');